/****************************************************************************
*  Title: corner_detect.sc
*  Author: Ashish Daga
*  Date: 04/16/2011
*  Description: takes in an 8bpp image array row by row over input c_queue
*			    and uses a Sobel mask for computing the differential gradients and then the Harris Response is 
			    computed. Response above the threshold are marked as corners whose locations are stored.
*
****************************************************************************/


#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>


// define image size, idea being from a hw camera this would be fixed
#define IMG_ROWS 480
#define IMG_CLMS 640

import "c_queue";

behavior harrisCorner( in unsigned char image8bpp[IMG_ROWS][IMG_CLMS],out double cornerOut[IMG_ROWS][IMG_CLMS])
{
//double cornerOut[IMG_ROWS][IMG_CLMS];
	double Gx[IMG_ROWS][IMG_CLMS];  //gradients used for calculating the response.
	double Gy[IMG_ROWS][IMG_CLMS];
	double Gxy[IMG_ROWS][IMG_CLMS];

	int PointX[IMG_ROWS];
	int PointY[IMG_CLMS];
	int Points_Count;

	// image buffers these are currently dependent on the size and format of the image used
	unsigned char cor_buffer[IMG_ROWS][IMG_CLMS];  

	int sobelX[3][3] = {{-1, 0, 1}, {-2, 0, 2}, {-1, 0, 1}};
	int sobelY[3][3] = {{1, 2, 1}, {0, 0, 0}, {-1, -2, -1}};
  // Harris parameters        
        //float k = 0.04f;
        float threshold = 50000;

        // Non-maximum suppression parameters
         int r = 3;

        // Gaussian smoothing parameters
         double sigma = 1.2;
         //float[] kernel;
         int size = 7;

         const float eps = 1.1920929E-07f;
   
	


	 int width = IMG_ROWS;
         int height = IMG_CLMS;   
	
	
	// detection variables
	int i, j,x, y,u,v,k,l,maxY,maxX,count;
	float currentValue;
	double min, max, luma;
	//Points_Count=0;

	//Map points 
 	float map[IMG_ROWS][IMG_CLMS];
		


	void main(void) 
	{		
		//********************
		// Perform corner detection
		//********************
		 
                //float* dx = diffx;
               // float* dy = diffy;
                //float* dxy = diffxy;
                //float* H = map;
                float M,A, B, C;

		//Smoothen Using Gaussian Filtering

      for (u = 1; u < width-1; u++) {
        for (v = 1; v < height-1; v++) {
            A=0;
            B=0;
            C=0;
            for(k=0;k<3;k++)
            {
                for(l=0;l<3;l++)
                {
                    A=A+image8bpp[u+k-1][v+l-1]*sobelX[k][l];   //differentials in the neighbourhood
                    B=B+image8bpp[u+k-1][v+l-1]*sobelX[k][l];	//differentials in the neighbourhood
                }

            }

            C=A*B;
            A=A*A;
            B=B*B;

			


  	map[u][v]=A*B-C*C-0.04*(A+B)*(A+B);


		if (map[u][v] > threshold)
                        {
                            // insert value in the map
			   cornerOut[y][x] = 0xFF;
                        }	

		}

	}
		
         
		// Non Max Suppression - 

            // for each row
            for ( y = r, maxY = height - r; y < maxY; y++)
            {
                // for each pixel
                for ( x = r, maxX = width - r; x < maxX; x++)
                {
                     currentValue = map[y][x];
		     //cornerOut[y][x] = 0x00;

                    // for each windows' row
                    for ( i = -r; (currentValue != 0) && (i <= r); i++)
                    {
                        // for each windows' pixel
                        for ( j = -r; j <= r; j++)
                        {
                            if (map[y + i][x + j] > currentValue)
                            {
                                currentValue = 0;
				//cornerOut[y][x] = 0x00;
                                break;
                            }
                        }
                    }

                    // check if this point is really interesting - then it is a corner
                    if (currentValue != 0)
                    {
                        //pointX[count] = x;  //mark the corner
			//pointY[count] = y; 
			//count++;
			cornerOut[y][x] = 0xFF;
			//printf(" Corner Points %d  %d  \n ", x,y);
			PointX[count] = x;
			PointX[count] = y;
			Points_Count ++;

                    }
                }
            }

	//OutQueue.send(cor_buffer[row], (IMG_CLMS));
                            
	printf(" Corner Points %d   \n ", Points_Count);
	}//end of main
	

};


/*
* Normalizes image of array double and sends out pixels over c_queue, assumes
* image pixels are magnitude only, no negative values
*/
behavior normalizeSender(in double diffxy[IMG_ROWS][IMG_CLMS], i_sender OutQueue) 
{
	int row, wt_cnt, i, j;
	double max, normFact;
	unsigned char edgeImage[IMG_ROWS][IMG_CLMS];
		
	void main(void) {
		
		// normalize not yet used 
		// find min an max value
		max = 0;
		//min = 0;
		for(i = 0; i < IMG_ROWS; i++) 
		{
			for(j = 0; j < IMG_CLMS; j++)
			{
				if (diffxy[i][j] > max) max = diffxy[i][j];
			}
		}
		
		normFact = max / 255;

		for(row = 0; row < IMG_ROWS; row++)
		{
			
			for(wt_cnt = 0; wt_cnt < IMG_CLMS; wt_cnt++)
			{
				edgeImage[row][wt_cnt] = (unsigned char) ((diffxy[row][wt_cnt]) / normFact);
			}
			
			// send image out
			OutQueue.send(edgeImage[row], (IMG_CLMS));
			//outImage[row] = edgeImage[row];
		}
		printf("Image Processor - Image has been sent out\n");
	}
};


/*
behavior cornerDetection(in unsigned char imageSmooth[IMG_ROWS][IMG_CLMS], 
		out unsigned char outImage[IMG_ROWS][IMG_CLMS])
{
	double cornerImg[IMG_ROWS][IMG_CLMS];

	harrisCorner	  corner(imageSmooth, cornerImg);
	normalizeSender   imgSender(cornerImg, outImage);

	void main(void)
	{
		corner.main();	
		imgSender.main();
	}

};
*/